﻿using ControlBeanExDll;
using OSSUtility.Communicate;
using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Controls;
using System.Windows;
using TcpserverExDll;
using static Ozone.Models.KeyPoint;
using System.Threading;

namespace Ozone.Device
{

    /// <summary>
    /// 左机械臂（液枪机械臂）
    /// </summary>
    public class DevRobot
    {
        public ControlBeanEx robot;
        public bool isbotInit = false;

        private int nRebootID = 0;
        /// <summary>
        /// 构造函数
        /// </summary>
        public DevRobot(int RebootID)
        {
            nRebootID = RebootID;

            TcpserverEx.net_port_initial();

        }

        /// <summary>
        /// 初始化机械臂
        /// </summary>
        /// <returns></returns>
        public bool Open()
        {
            bool isbotInit = false;
            robot = TcpserverEx.get_robot(nRebootID);//替换为自己的机器的id号
            if (robot.is_connected())
            {
                if (robot == null) return isbotInit;
                robot = TcpserverEx.get_robot(nRebootID);
                if (robot.is_connected())
                {
                    bool b = robot.check_joint(4, false);
                    int ret = robot.initial(1, 210);
                    //ret==3机械臂当前位置不在限定范围使关节强制回零
                    //先手动调整
                    if (ret == 3)
                    {
                        MessageBox.Show("robot" + "不在目标轨迹内");
                        //使关节强制回零
                        //robot.joint_home(3);
                        //robot.joint_home(4);
                        //robot.joint_home(1);
                        //robot.joint_home(2);

                        ////初始化
                        //ret = robot.initial(1, 210);
                    }
                    if (ret == 1)
                    {
                        robot.unlock_position();
                        isbotInit = true;
                    }
                    else
                    {
                        MessageBox.Show("robot" + "初始化失败，返回值 = " + ret.ToString());
                    }
                }
                else
                {
                    MessageBox.Show("robot" + "未连接");
                }
                isbotInit = true;
            }
            return isbotInit;
        }




        /// <summary>
        /// 直线模式运动到指定点位 （注意手系的变换）
        /// </summary>
        /// <param name="pointXYZ"></param>
        /// <param name="speed"></param>
        /// <returns></returns>
        public bool my_move_pointxyz(PointXYZR pointXYZ, float speed)
        {
            bool BrginMove = false;
            //X坐标
            float x = float.Parse(pointXYZ.X.ToString());
            //X坐标
            float y = float.Parse(pointXYZ.Y.ToString());
            //X坐标
            float z = float.Parse(pointXYZ.Z.ToString());
            int lr = pointXYZ.Lr;
            //以 movej（直线模式） 的运动方式，并以指定手系达目标点
            int ret = robot.new_movej_xyz_lr(x,
                                    y,
                                    z,
                                    robot.rotation,
                                    speed,
                                    0,
                                    lr);
            if (ret == 1)
            {
                BrginMove = true;
            }
            robot.wait_stop();
            Thread.Sleep(100);
            return BrginMove;
        }

        /// <summary>
        /// 直线模式运动到指定点位 （注意手系的变换主要用于拧瓶盖）
        /// </summary>
        /// <param name="pointXYZ"></param>
        /// <param name="speed"></param>
        /// <returns></returns>
        public bool my_move_pointxyzr(PointXYZR pointXYZ, float speed)
        {
            bool BrginMove = false;
            //X坐标
            float x = float.Parse(pointXYZ.X.ToString());
            //X坐标
            float y = float.Parse(pointXYZ.Y.ToString());
            //X坐标
            float z = float.Parse(pointXYZ.Z.ToString());
            //R角度
            float r = float.Parse(pointXYZ.R.ToString());
            int lr = pointXYZ.Lr;
            //以 movej（直线模式） 的运动方式，并以指定手系达目标点
            int ret = robot.new_movej_xyz_lr(x,
                                    y,
                                    z,
                                    r,
                                    speed,
                                    0,
                                    lr);
            if (ret == 1)
            {
                BrginMove = true;
            }
            robot.wait_stop();
            Thread.Sleep(100);
            return BrginMove;
        }

 

    }
}
